package com.hitqz.robot.biz.flow.component.oilSample;

import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.hitqz.robot.api.business.enums.BrandEnum;
import com.hitqz.robot.api.business.enums.EquipmentEnum;
import com.hitqz.robot.api.business.enums.RobotOtherCommandEnum;
import com.hitqz.robot.api.common.dto.robot.RobotMoveDto;
import com.hitqz.robot.api.common.entity.RobotEquipment;
import com.hitqz.robot.biz.service.RobotEquipmentService;
import com.hitqz.robot.camera.EquipmentFactory;
import com.hitqz.robot.camera.IEquipmentService;
import com.hitqz.robot.camera.dto.EquipmentParamsDto;
import com.hitqz.robot.camera.dto.QueryParamsDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import com.yomahub.liteflow.annotation.LiteflowComponent;
import com.yomahub.liteflow.core.NodeComponent;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;

import java.util.List;
import java.util.Optional;
import java.util.concurrent.TimeUnit;

/**
 * @author xupkun
 * @date 2024/11/14
 */
@LiteflowComponent(value = "ForwardAdjustComponent", name = "[取油样机器人]前进后退调整")
@RequiredArgsConstructor
@Slf4j
public class ForwardAdjustComponent extends NodeComponent {


    private final RobotEquipmentService robotEquipmentService;

    private final RobotDriverService robotDriverService;

    private final EquipmentFactory equipmentFactory;



    @Override
    public void process() throws Exception {

        TimeUnit.SECONDS.sleep(3);
        List<RobotEquipment> carPlc = robotEquipmentService.list(Wrappers.<RobotEquipment>lambdaQuery()
                //.eq(RobotEquipment::getType, EquipmentEnum.PLC.getType())
                .eq(RobotEquipment::getType, EquipmentEnum.CAR_PLC.getType())
                .eq(RobotEquipment::getBrand, BrandEnum.CAR_PLC.getEnStr())
                .eq(RobotEquipment::getIsRecordParam, 1));

        RobotEquipment robotEquipment = carPlc.get(0);
        QueryParamsDto queryParamsDto = new QueryParamsDto();
        queryParamsDto.setPassword(robotEquipment.getPassword());
        queryParamsDto.setUsername(robotEquipment.getUsername());
        queryParamsDto.setPort(robotEquipment.getPort());
        queryParamsDto.setChannel(robotEquipment.getChannel());
        queryParamsDto.setIp(robotEquipment.getIp());
        IEquipmentService service = equipmentFactory.getServiceByBrand(robotEquipment.getBrand());
        while (true) {
            List<EquipmentParamsDto> equipmentParamsDtos = service.getEquipmentParams(queryParamsDto);
            Optional<EquipmentParamsDto> leftDistance = equipmentParamsDtos.stream().filter(t->t.getKey().equals("leftDistance")).findFirst();
            Optional<EquipmentParamsDto> rightDistance = equipmentParamsDtos.stream().filter(t->t.getKey().equals("rightDistance")).findFirst();
            if (leftDistance.isPresent() && rightDistance.isPresent()) {
                Integer left = Integer.parseInt(leftDistance.get().getValue());
                Integer right = Integer.parseInt(rightDistance.get().getValue());
                log.info("Left: " + left + ", Right: " + right);
                if (left >= 100 && right >= 100) {
                    log.info("允许地盘前后微调");
                    robotDriverService.executeOtherCommand(RobotOtherCommandEnum.ROD_CONTROLLER, "");
                    Thread.sleep(1000);
                    Thread thread = new Thread(() -> {
                        while (true) {
                            try {
                                Thread.sleep(250);
                            } catch (InterruptedException e) {
                                throw new RuntimeException(e);
                            }
                            log.info("set speed");
                            setSpeed("0.01", "0", null);
                        }
                    });
                    thread.start();
                    Thread.sleep(500);
                    setSpeed("0", "0", "0");
                    log.info("thread run stopped");
                    thread.stop();
                    Thread.sleep(3000);
                } else {
                    break;
                }
            }
        }
    }

    private void setSpeed(String linerSpeed, String linerSpeedY, String angularSpeed) {
        RobotMoveDto robotMoveDto = new RobotMoveDto();
        robotMoveDto.setLinerSpeed(linerSpeed);
        robotMoveDto.setLinerSpeedY(linerSpeedY);
        robotMoveDto.setAngularSpeed(angularSpeed);
        robotDriverService.setSpeed(robotMoveDto);
    }
}
